//internal operators
template <typename scalar>
Matrix_4x4<scalar>& Matrix_4x4<scalar> :: operator *= (const Matrix_4x4<scalar>& p)
{
	scalar result[4][4];
	for (mInt32 i = 0; i<4; i++)
      for (mInt32 j = 0; j<4; j++)
	  {
		  result[i][j] = 0;
		  for (mInt32 k = 0; k<4; k++)
			  result[i][j] += elements[4*i+k] * p(k, j);
	  };
	for (mInt32 i = 0; i<4; i++)
      for (mInt32 j = 0; j<4; j++)
	  {
		  elements[4*i+j] = result[i][j];
  	  };
	return *this;
}

template <typename scalar>
Matrix_4x4<scalar>& Matrix_4x4<scalar> :: operator += (const Matrix_4x4<scalar>& p)
{
	for (mInt32 i = 0; i < 16; i++)
	{
		elements[i] += p.elements[i];
	}
	return *this;
}

template <typename scalar>
Matrix_4x4<scalar>& Matrix_4x4<scalar> :: operator -= (const Matrix_4x4<scalar>& p)
{
	for (mInt32 i = 0; i < 16; i++)
	{
		elements[i] -= p.elements[i];
	}
	return *this;
}

template <typename scalar>
mBool Matrix_4x4<scalar> :: operator == (const Matrix_4x4<scalar>& p)
{
  for (mInt32 i = 0; i < 16; ++i)
    if (elements[i] != p.elements[i]) return 0;
  return 1;
}

template <typename scalar>
mBool Matrix_4x4<scalar> :: operator != (const Matrix_4x4<scalar>& p)
{
  for (mInt32 i = 0; i < 16; ++i)
    if (elements[i] != p.elements[i]) return 1;
  return 0;
}




//external operators
template <typename scalar>
Matrix_4x4<scalar> operator * (const Matrix_4x4<scalar>& a, const Matrix_4x4<scalar>& b)
{
    mInt32 i, j, k;
	Matrix_4x4<scalar> result;
	for (i = 0; i < 4; i++)
      for (j = 0; j < 4; j++)
	  {
		  result(i, j) = 0;
		  for (k = 0; k < 4; k++)
			  result(i, j) += a(i, k) * b(k, j);
	  };
	return result;
}

template <typename scalar>
vect::Vector_3<scalar> operator * (const vect::Vector_3<scalar> v, const Matrix_4x4<scalar>& m)
{
    vect::Vector_3<scalar> result;
    result.x = v.x * m.elements[0] + v.y * m.elements[4] + v.z * m.elements[8] + m.elements[12];
    result.y = v.x * m.elements[1] + v.y * m.elements[5] + v.z * m.elements[9] + m.elements[13];
    result.z = v.x * m.elements[2] + v.y * m.elements[6] + v.z * m.elements[10] + m.elements[14];
	return result;
}

template <typename scalar>
vect::Vector_4<scalar> operator * (const vect::Vector_4<scalar> v, const Matrix_4x4<scalar>& m)
{
	return vect::Vector_4<scalar>(v.x * m(0, 0) + v.y * m(1, 0) + v.z * m(2, 0) + v.t * m(3, 0),
                          v.x * m(0, 1) + v.y * m(1, 1) + v.z * m(2, 1) + v.t * m(3, 1),
                          v.x * m(0, 2) + v.y * m(1, 2) + v.z * m(2, 2) + v.t * m(3, 2),
                          v.x * m(0, 3) + v.y * m(1, 3) + v.z * m(2, 3) + v.t * m(3, 3));
}

template <typename scalar>
vect::Vector_4<scalar> operator * (const Matrix_4x4<scalar>& m, const vect::Vector_4<scalar> v)
{
	return vect::Vector_4<scalar>(v.x * m(0, 0) + v.y * m(0, 1) + v.z * m(0, 2) + v.t * m(0, 3),
                          v.x * m(1, 0) + v.y * m(1, 1) + v.z * m(1, 2) + v.t * m(1, 3),
                          v.x * m(2, 0) + v.y * m(2, 1) + v.z * m(2, 2) + v.t * m(2, 3),
                          v.x * m(3, 0) + v.y * m(3, 1) + v.z * m(3, 2) + v.t * m(3, 3));
}

//constructors
template <typename scalar>
Matrix_4x4<scalar> :: Matrix_4x4()
{
	for (mInt32 i = 0; i<4; i++)
	{
		for (mInt32 j = 0; j<4; j++)
		{
			elements[4*i+j] = (i==j) ? scalar(1) : scalar(0);
		};
	}
}

template <typename scalar>
Matrix_4x4<scalar> :: Matrix_4x4(Matrix_4x4<scalar>& m)
{
	mInt32 i;
    for (i = 0; i < 16; ++i)
    {
        elements[i] = m.elements[i];
    }
}
template <typename scalar>
Matrix_4x4<scalar> :: Matrix_4x4(scalar *newElements)
{
  for (mInt32 i = 0; i < 16; i++)
	{
    elements[i] = newElements[i];
  }

}

template <typename scalar>
Matrix_4x4<scalar> :: Matrix_4x4(const Vector& t, const Vector& r, const Vector& S, mFloat angle)
{
//VECTOR r MUST BE OF THE UNIT LENGTH!!!!!!!!!!!!!!!!!!!!!
//rotation
	mFloat rad_angle = com::grad2rad(angle);
	mFloat s = sin(rad_angle);
	mFloat c = cos(rad_angle);
  elements[0] = c + (1 - c)* r.x * r.x;//0, 0
  elements[1] = (1 - c)* r.y * r.x- s * r.z;//0, 1
  elements[2] = (1 - c) * r.z * r.x + s * r.y;//0, 2
  elements[3] = 0;//0, 3
//++++++++++++++++++++++++++++++
  elements[4] = (1 - c) * r.x * r.y + s * r.z ;//1, 0
  elements[5] = c + (1 - c) * r.y * r.y;//1, 1
  elements[6] = (1 - c) * r.z * r.y - s * r.x;//1, 2
  elements[7] = 0;//1, 3
//++++++++++++++++++++++++++++++
  elements[8] = (1 - c) * r.x * r.z - s * r.y ;//2, 0
  elements[9] = (1 - c) * r.y * r.z + s * r.x;//2, 1
  elements[10] = c + (1 - c) * r.z * r.z;//2, 2
  elements[11] = 0;//2, 3
//++++++++++++++++++++++++++++++
//  *this(3, 0) = 0;//
//  *this(3, 1) = 0;//
//  *this(3, 2) = 0;//
  elements[15] = 1;//3, 3
//++++++++++++++++++++++++++++++
//translation
  elements[12] = t.x;//3, 0
  elements[13] = t.y;//3, 1
  elements[14] = t.z;//3, 2
//scale
  for (mInt32 i = 0; i < 3; i++)
  {
	  elements[5*i] *= S[i];
  };
}

//functions
template <typename scalar>
vect::Vector_4<scalar> Matrix_4x4<scalar> :: GetRow(mInt32 num)
{
  return vect::Vector_4<scalar>(elements[num*4], elements[num*4 + 1], elements[num*4 + 2], elements[num*4 + 3]);
}

template <typename scalar>
void Matrix_4x4<scalar> :: RotateAround(Vector& r, mFloat angle)
{
  r.Normalize();
	mFloat rad_angle = com::grad2rad(angle);
	mFloat s = sin(rad_angle);
	mFloat c = cos(rad_angle);
  Matrix_4x4<scalar> RM;
  RM(0, 0) = c + (1 - c)* r.x * r.x;
  RM(0, 1) = (1 - c)* r.y * r.x- s * r.z;
  RM(0, 2) = (1 - c) * r.z * r.x + s * r.y;
  //RM(0, 3) = 0;
//++++++++++++++++++++++++++++++
  RM(1, 0) = (1 - c) * r.x * r.y + s * r.z ;
  RM(1, 1) = c + (1 - c) * r.y * r.y;
  RM(1, 2) = (1 - c) * r.z * r.y - s * r.x;
  //RM(1, 3) = 0;
//++++++++++++++++++++++++++++++
  RM(2, 0) = (1 - c) * r.x * r.z - s * r.y ;
  RM(2, 1) = (1 - c) * r.y * r.z + s * r.x;
  RM(2, 2) = c + (1 - c) * r.z * r.z;
  //RM(2, 3) = 0;
  *this *= RM;
}

template <typename scalar>
void GenRotateMatrix(Matrix_4x4<scalar> *a_ResMtx, vect::Vector_3<scalar> &a_Axis, scalar a_Angle)
{
    vect::Vector_3<scalar> r = a_Axis;
    r.Normalize();
    scalar rad_angle = com::grad2rad(a_Angle);
    scalar s = sin(rad_angle);
    scalar c = cos(rad_angle);
    
    (*a_ResMtx)(0, 0) = c + (1 - c)* r.x * r.x;
    (*a_ResMtx)(0, 1) = (1 - c)* r.y * r.x- s * r.z;
    (*a_ResMtx)(0, 2) = (1 - c) * r.z * r.x + s * r.y;
    //(*a_ResMtx)(0, 3) = 0;
    //++++++++++++++++++++++++++++++
    (*a_ResMtx)(1, 0) = (1 - c) * r.x * r.y + s * r.z ;
    (*a_ResMtx)(1, 1) = c + (1 - c) * r.y * r.y;
    (*a_ResMtx)(1, 2) = (1 - c) * r.z * r.y - s * r.x;
    //(*a_ResMtx)(1, 3) = 0;
    //++++++++++++++++++++++++++++++
    (*a_ResMtx)(2, 0) = (1 - c) * r.x * r.z - s * r.y ;
    (*a_ResMtx)(2, 1) = (1 - c) * r.y * r.z + s * r.x;
    (*a_ResMtx)(2, 2) = c + (1 - c) * r.z * r.z;
    //(*a_ResMtx)(2, 3) = 0;
}

template <typename scalar>
Matrix_4x4<scalar>& Matrix_4x4<scalar>::Transpose()
{
    mInt32 i, j;
    scalar temp;
    for (i = 0; i < 4; i++)
        for (j = 0; j < i; j++)
        {
            temp = elements[4 * i + j];
            elements[4 * i + j] = elements[4 * j + i];
            elements[4 * j + i] = temp;
        }
    return *this;
}

template <typename scalar>
Matrix_4x4<scalar>& Matrix_4x4<scalar>::operator !() const
{
  Matrix_4x4<scalar> temp1;
  static Matrix_4x4<scalar> res;
  for (mInt32 i = 0; i < 4; i++)
  {
    for (mInt32 j = 0; j < 4; j++)
    {
      temp1 = *this;
      for (mInt32 k = 0; k < 4; k++)
      {
        temp1(i, k) = 0;
        temp1(k, j) = 0;
      }
      temp1(i, j) = 1;
      res(j, i) = /*(((i + j + 1) % 2) * 2 - 1) */ temp1.Det() / Det();
    }
  }
  return res;
}

template <typename scalar>
scalar Matrix_4x4<scalar>::Det() const
{
  return (elements[0] * (elements[5] * (elements[10] * elements[15] - elements[11] * elements[14]) -
                        elements[6] * (elements[9] * elements[15] - elements[11] * elements[13]) +
                        elements[7] * (elements[9] * elements[14] - elements[10] * elements[13])) -
         elements[1] * (elements[4] * (elements[10] * elements[15] - elements[11] * elements[14]) -
                        elements[6] * (elements[8] * elements[15] - elements[11] * elements[12]) +
                        elements[7] * (elements[8] * elements[14] - elements[10] * elements[12])) +
         elements[2] * (elements[4] * (elements[9] * elements[15] - elements[11] * elements[13]) -
                        elements[5] * (elements[8] * elements[15] - elements[11] * elements[12]) +
                        elements[7] * (elements[8] * elements[13] - elements[9] * elements[12])) -
         elements[3] * (elements[4] * (elements[9] * elements[14] - elements[10] * elements[13]) -
                        elements[5] * (elements[8] * elements[14] - elements[10] * elements[12]) +
                        elements[6] * (elements[8] * elements[13] - elements[9] * elements[12])));
}